#! /usr/bin/env python
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
import rospy
from std_msgs.msg import String,Int32MultiArray
from sensor_msgs.msg import Image

class Match:
    def __init__(self):
        self.bridge = CvBridge()
        self.img_sub = rospy.Subscriber('/camera/rgb/image_raw',Image,self.imgCallback)
        self.location_pub = rospy.Publisher('/camera/loc',Int32MultiArray,queue_size=10)
        self.template = cv2.imread("image_coke.png")
        self.std = 0
        
        h1, w1 = self.template.shape[:2]
        self.loc = [0,0,h1,w1]
        #print("h1={0}, w1={0}".format(h1, w1))
        #self.template = cv2.resize(self.template, (int(h1*1), int(w1*1)))
        # self.match(1)

    def imgCallback(self,data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data,'bgr8')
            self.std = 0
            # cv2.imshow('src',cv_image)
            # cv2.waitKey(1)
            self.match(cv_image)
            if self.std == 1:
                print('capture')
                msg = Int32MultiArray(data = self.loc)
                # print(msg)
                self.location_pub.publish(msg)

        except:
            print('error')


    def match(self, data):
        
        img = data
        # img = cv2.imread("1.jpg")
        #cv2.imshow("template", self.template)
        h1,w1 = self.template.shape[:2]
        h2, w2 = img.shape[:2]

        print("h1={0}, w1={0}".format(h1, w1))
        print('h2={0}, w2={0}'.format(h2, w2))

        result = cv2.matchTemplate(img, self.template, cv2.TM_CCOEFF_NORMED)
        min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)
        top_left = max_loc
        self.loc[0] = top_left[0]
        self.loc[1] = top_left[1]
        bottom_right = (top_left[0] + self.template.shape[1], top_left[1] + self.template.shape[0])
        self.loc[2] = bottom_right[0]
        self.loc[3] = bottom_right[1]
        cv2.rectangle(img, top_left, bottom_right, (255, 0, 0), 2)
        cv2.imshow("img", img)
        if cv2.waitKey(1)==ord('c'):
            self.std = 1

def main():
    rospy.init_node('select_target',anonymous = False)
    match = Match()
    rospy.spin()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass
